package test;
import java.io.DataInputStream;

import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.Sound;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;


public class OmniWheelNxt {

	public static void main(String[] args) {
		try {
			new OmniWheelNxt();
		} catch (Exception e) {
		}
	}

	private BTConnection btc;
	private DataInputStream dis;

	public OmniWheelNxt() throws Exception{
		String connected = "Connected";
		String waiting = "Waiting for connection";

		Button.ESCAPE.addButtonListener(new ButtonListener() {

			@Override public void buttonReleased(Button arg0) { }

			@Override
			public void buttonPressed(Button arg0) {
				cleanup();
				System.exit(0);
			}
		});

		while (true)
		{
			LCD.drawString(waiting,0,0);
			LCD.refresh();

			//wait for connection from something
			btc = Bluetooth.waitForConnection();
			Sound.beep();

			LCD.clear();
			LCD.drawString(connected,0,0);
			LCD.refresh();	

			//Bt data Stream
			dis = btc.openDataInputStream();

			char motor;
			int power;
			while (! Button.ESCAPE.isPressed() )
			{
				motor = dis.readChar();
				power = dis.readInt();
//				System.out.println(motor + " " + power);

				if(motor == 'A'){
					controlMotor(Motor.A, power);
//					LCD.drawInt(power, 0, 0);
				}
				else if(motor == 'B'){
					controlMotor(Motor.B, power);
//					LCD.drawInt(power, 0, 1);
				}
				else if(motor == 'C'){
					controlMotor(Motor.C, power);
//					LCD.drawInt(power, 0, 2);
				}
//				LCD.refresh();
			}

			cleanup();
		}
	}
	
	private void controlMotor(Motor motor, int power){
		motor.setSpeed(power);
		//1 = forward, 2= backward, 3 = stop, 4 = float
		if(power == 0) {
			motor.stop();
		}
		else if(power > 0) {
			motor.forward();
		}
		else{
			motor.backward();
		}
	}

	private void cleanup(){
		try{
			dis.close();
			Thread.sleep(100); // wait for data to drain
			btc.close();
			LCD.clear();
		}
		catch (Exception e) {
		}
	}
}
